#include "RampArm.h"
#include "../Robotmap.h"
#include "../Commands/RampArmControl.h"

RampArm::RampArm() : Subsystem("RampArm") {
	
	rampArmMotor = new Jaguar(RobotMap::OTHER_RAMP_ARM_MOTOR);
	
	frontLimit = new DigitalInput(RobotMap::FEED_RAMP_ARM_DOWN);
	
	rearLimit = new DigitalInput(RobotMap::FEED_RAMP_ARM_UP);
	
}

void RampArm::InitDefaultCommand()
{
	SetDefaultCommand(new RampArmControl());
}

void RampArm::moveArm(float speed) {
	
	if((isArmInForwardPosition() && (getOriginalSign(speed) < 0))
		|| (isArmInBackwardPositioin() && (getOriginalSign(speed) > 0)))
		rampArmMotor->StopMotor();
	else
		rampArmMotor->SetSpeed(speed);
	
}


bool RampArm::isArmInForwardPosition() {
	return !frontLimit->Get();
}

bool RampArm::isArmInBackwardPositioin() {
	return !rearLimit->Get();
}

float RampArm::getOriginalSign(float x) {
	return x < 0.0 ? -1.0 : 1.0;
}
